#include "Algorithm/subdivision_of_polygon.h"



static double compute_score_in_polygon(double**v,int len,double* n,int i)
{
    int j=(i-1+len)%len,k=(i+1)%len;
    double a[3]={v[i][0]-v[j][0],v[i][1]-v[j][1],v[i][2]-v[j][2]};
    double b[3]={v[k][0]-v[i][0],v[k][1]-v[i][1],v[k][2]-v[i][2]};
    if(!normalize(a,3)||!normalize(b,3))
    {
        return -1;
    }
    double n1[3]={0};
    out_productn(a,b,n1);
    double score=inner_product(a,b,3);
    if(inner_product(n1,n,3)<0)
    {
        score=score-2;
    }
    else
    {
        score=-score;
    }
    //free(n1);
    if(score<0){return score;}
    double tn1[3]={0},tn2[3]={0},tn3[3]={0};
    for(int ii=0;ii<len;ii++)
    {
        if(ii==i||ii==j||ii==k){continue;}
        double dir1[3]={v[ii][0]-v[j][0],v[ii][1]-v[j][1],v[ii][2]-v[j][2]};
        double dir2[3]={v[ii][0]-v[i][0],v[ii][1]-v[i][1],v[ii][2]-v[i][2]};
        double dir3[3]={v[ii][0]-v[k][0],v[ii][1]-v[k][1],v[ii][2]-v[k][2]};
        out_productn(dir1,dir2,tn1);
        out_productn(dir2,dir3,tn2);
        out_productn(dir3,dir1,tn3);
        if(inner_product(tn1,tn2,3)>=0&&inner_product(tn1,tn3,3)>=0
            &&inner_product(tn2,tn3,3)>=0)
        {
            score=-1;
        }
        //free(tn1);free(tn2);free(tn3);
        if(score<0){break;}

    }
    return score;
}





typedef  struct Sub_Poly_Ver{
    double* p;
    int i;
    double score;
    int update;
}Sub_Poly_Ver;

static inline void sub_poly_ver_init(Sub_Poly_Ver* spv)
{
    spv->p=NULL;
    spv->i=0;
    spv->score=-2;
    spv->update=1;
}

static double compute_one_angle(double *p1,double* p2,double*p3,double* n)
{
    double dir1[3]={p3[0]-p2[0],p3[1]-p2[1],p3[2]-p2[2]};
    double dir2[3]={p1[0]-p2[0],p1[1]-p2[1],p1[2]-p2[2]};

    if(!normalize(dir1,3)||!normalize(dir2,3))
    {
        return 0;
    }
    double n1[3]={0};
    out_productn(dir1,dir2,n1);
    double score=inner_product(dir1,dir2,3);
    score=1-score;
    if(inner_product(n1,n,3)<0)
    {
        score=score-2;
    }
    //free(n1);
    return score;
}

static double compute_score_in_polygon1(Sub_Poly_Ver* spvs,int len,double* n,int i)
{
    int trian[3]={0};
    trian[0]=i; trian[1]=(i+1)%len;trian[2]=(i-1+len)%len;
    double score=compute_one_angle(spvs[trian[0]].p,spvs[trian[1]].p,spvs[trian[2]].p,n);

    if(score<0){return score;}

    trian[0]=(i+1)%len; trian[1]=(i-1+len)%len;trian[2]=i;
    double temp_score=compute_one_angle(spvs[trian[0]].p,spvs[trian[1]].p,spvs[trian[2]].p,n);
    if(temp_score<score)
    {
        score=temp_score;
    }
    if(score<0){return score;}
    trian[0]=(i-1+len)%len; trian[1]=(i+1)%len;trian[2]=(i+2)%len;
    temp_score=compute_one_angle(spvs[trian[0]].p,spvs[trian[1]].p,spvs[trian[2]].p,n);
    if(temp_score<score)
    {
        score=temp_score;
    }
    if(score<0){return score;}

    trian[0]=(i-2+len)%len; trian[1]=(i-1+len)%len;trian[2]=(i+1)%len;

    temp_score=compute_one_angle(spvs[trian[0]].p,spvs[trian[1]].p,spvs[trian[2]].p,n);
    if(temp_score<score)
    {
        score=temp_score;
    }
    if(score<0){return score;}
    double tn1[3]={0},tn2[3]={0},tn3[3]={0};
    int j=(i-1+len)%len,k=(i+1)%len;
    for(int ii=0;ii<len;ii++)
    {
        if(ii==i||ii==j||ii==k||
            ii==(i-2+len)%len||ii==(i+2)%len
            ){continue;}
        double dir1[3]={spvs[ii].p[0]-spvs[j].p[0],spvs[ii].p[1]-spvs[j].p[1],
            spvs[ii].p[2]-spvs[j].p[2]};
        double dir2[3]={spvs[ii].p[0]-spvs[i].p[0],spvs[ii].p[1]-spvs[i].p[1],
            spvs[ii].p[2]-spvs[i].p[2]};
        double dir3[3]={spvs[ii].p[0]-spvs[k].p[0],spvs[ii].p[1]-spvs[k].p[1],
            spvs[ii].p[2]-spvs[k].p[2]};
        out_productn(dir1,dir2,tn1);
        out_productn(dir2,dir3,tn2);
        out_productn(dir3,dir1,tn3);
        if(inner_product(tn1,tn2,3)>=0&&inner_product(tn1,tn3,3)>=0
            &&inner_product(tn2,tn3,3)>=0)
        {
            score=-1;
        }
        //free(tn1);free(tn2);free(tn3);
        if(score<=-1){break;}
    }
    return score;
}
static void subdivision_of_polygon1( Sub_Poly_Ver* spvs,int len,int **s,double* n)
{
    if(len<3)
    {
        return ;
    }
    if(len==3)
    {
        s[0][0]=spvs[0].i;
        s[0][1]=spvs[1].i;
        s[0][2]=spvs[2].i;
        return ;
    }


    // double** v=(double**)malloc(sizeof(double*)*len);
    // for(int i=0;i<len;i++)
    // {
    //     v[i]=spvs[i].p;
    //    // printf("%lf %lf %lf\n",v[i][0],v[i][1],v[i][2]);
    // }
    //double *scores=(double*)malloc(sizeof(double)*len);
    int an=-1;double temp_score=-2;
    for(int i=0;i<len;i++ )
    {

        if(spvs[i].update==1)
        {
            spvs[i].score =compute_score_in_polygon1(spvs,len, n,i);
            spvs[i].update=0;
        }
        // printf("spvs i:%d score:%lf\n",spvs[i].i,score);

        if(spvs[i].score>temp_score)
        {
            an=i;
            temp_score=spvs[i].score;
        }
    }

    // free(v);
    //free(scores);
    //printf("an:%d\n",an);

    if(an==-1){return;}

   // printf("here1\n");
    int ia=(an-1+len)%len,ib=(an+1+len)%len;
    s[0][0]=spvs[ia].i;
    s[0][1]=spvs[an].i;
    s[0][2]=spvs[ib].i;

    spvs[ia].update=1;spvs[ib].update=1;

    for(int i=an;i<len-1;i++)
    {
        spvs[i]=spvs[i+1];
    }
    // int temp_i=0;
    // Sub_Poly_Ver* spvs1=(Sub_Poly_Ver*)malloc(sizeof(Sub_Poly_Ver)*(len-1));
    // for(int i=0;i<len;i++)
    // {
    //     if(i==an)
    //     {
    //         continue;
    //     }
    //     spvs1[temp_i]=spvs[i];
    //     temp_i++;
    // }
    subdivision_of_polygon1(spvs,len-1,&(s[1]),n);
    // free(spvs1);
}


void subdivision_of_polygon(double ** v,int len,int ** s)
{
    double n[3]={0};
    compute_plane_normal_of_jordan_curve(v,len,n);

    Sub_Poly_Ver* spvs=(Sub_Poly_Ver*)malloc(sizeof(Sub_Poly_Ver)*len);
    for(int i=0;i<len;i++)
    {
        sub_poly_ver_init(&(spvs[i]));
        spvs[i].p=v[i];
        spvs[i].i=i;
    }

    subdivision_of_polygon1(spvs,len,s,n);
    free(spvs);
    // SAFE_FREE(n);
}
static inline int is_three_point_in_one_line(double* p1,double* p2,double* p3)
{
    //assert(false);
    double temp_p1[3]={p2[0]-p1[0],p2[1]-p1[1],p2[2]-p1[2]};
    double temp_p2[3]={p3[0]-p1[0],p3[1]-p1[1],p3[2]-p1[2]};
    //normalize
    double p[3]={0};
    out_productn(temp_p1,temp_p2,p);
    int re=0;
    if(inner_product(temp_p1,temp_p2,3)<0 &&lb_norm(p,3)<1e-8)
    {re=1;}

    //SAFE_FREE(p);
    return re;
}
// p1 p2 p3按顺序

static inline int is_opt_sub_opt_vertex(double* p1,double* p2,double* p3,double* n)
{
    double temp_p1[3]={p1[0]-p2[0],p1[1]-p2[1],p1[2]-p2[2]};
    double temp_p2[3]={p3[0]-p2[0],p3[1]-p2[1],p3[2]-p2[2]};
    //normalize
    double p[3]={0};
    out_productn(temp_p2,temp_p1,p);
    int re=0;
    if(inner_product(p,n,3)<0)
    {
        re=0;
    }
    else{
        if(!normalize(temp_p1,3)||!normalize(temp_p2,3))
        {
            re=1;
        }
        else if(inner_product(temp_p1,temp_p2,3)<-0.98)
        {
            re=1;
        }
    }

    //SAFE_FREE(p);
    return re;
}

static void optimization_subdivision_of_polygon1( Sub_Poly_Ver* spvs,int len,int **s,double* n)
{
    if(len<3){return;}
    if(len==3){
        s[0][0]=spvs[0].i;
        s[0][1]=spvs[1].i;
        s[0][2]=spvs[2].i;
        return ;
    }

    Sub_Poly_Ver spv,spv1,spv2;
    spv.i=-1;spv1.i=-1,spv2.i=-1;

    if(spv.i==-1)
    {
        for(int i=0;i<len;i++)
        {
            if(is_opt_sub_opt_vertex( spvs[(i-1+len)%len].p,spvs[i].p,spvs[(i+1)%len].p,n) )
            {
                spv2=spvs[(i-1+len)%len];
                spv=spvs[i];
                spv1=spvs[(i+1)%len];
                break;
            }
        }
    }

    if(spv.i==-1)
    {
        subdivision_of_polygon1(spvs,len,s,n);
    }
    else
    {
        //printf("opt sub i:%d %d %d\n",spv.i,spv1.i,spv2.i);
        Sub_Poly_Ver* spvs1=(Sub_Poly_Ver*)malloc(sizeof(Sub_Poly_Ver)*(len-1));
        int temp_i=0;
        for(int i=0;i<len;i++)
        {
            if(spvs[i].i!=spv.i)
            {
                spvs1[temp_i]=spvs[i];
                temp_i++;
            }
        }

        optimization_subdivision_of_polygon1(spvs1,len-1,s,n);

        for(int i=0;i<len-3;i++)
        {
            if(s[i][0]==spv2.i&&s[i][1]==spv1.i)
            {
                s[len-3][0]=spv.i;s[len-3][1]=s[i][1];s[len-3][2]=s[i][2];

                s[i][1]=spv.i;

                break;
            }
            else if(s[i][1]==spv2.i&&s[i][2]==spv1.i)
            {
                s[len-3][0]=s[i][0];s[len-3][1]=spv.i;s[len-3][2]=s[i][2];

                s[i][2]=spv.i;

                break;
            }
            else if(s[i][2]==spv2.i&&s[i][0]==spv1.i)
            {
                s[len-3][0]=s[i][0];s[len-3][1]=s[i][1];s[len-3][2]=spv.i;

                s[i][0]=spv.i;

                break;
            }
        }
        free(spvs1);
    }
}

static int is_edge_inter_with_triangle(Sub_Poly_Ver* edge,Sub_Poly_Ver spv1,
    Sub_Poly_Ver spv2,Sub_Poly_Ver spv3,double* n)
{
    if((edge[0].i==spv1.i&&edge[1].i==spv2.i)||(edge[1].i==spv1.i&&edge[0].i==spv2.i) )
    {
        return 2;
    }
    else if((edge[0].i==spv2.i&&edge[1].i==spv3.i)||(edge[1].i==spv2.i&&edge[0].i==spv3.i))
    {
        return 2;
    }
    else if((edge[0].i==spv1.i&&edge[1].i==spv3.i)||(edge[1].i==spv1.i&&edge[0].i==spv3.i))
    {
        return 2;
    }
    else if(edge[0].i==spv1.i||edge[0].i==spv2.i||edge[0].i==spv3.i||
        edge[1].i==spv1.i||edge[1].i==spv2.i||edge[1].i==spv3.i)
    {
        return 1;
    }

    double tn1[3]={0},tn2[3]={0},tn3[3]={0};
    double l[6]={0};int re=0;
    if(get_line_equation_from_2planes(edge[0].p,edge[1].p ,l)==0)
    {
        double dir1[3]={edge[0].p[0]-spv1.p[0],edge[0].p[1]-spv1.p[1],edge[0].p[2]-spv1.p[2]};
        double dir2[3]={edge[0].p[0]-spv2.p[0],edge[0].p[1]-spv2.p[1],edge[0].p[2]-spv2.p[2]};
        double dir3[3]={edge[0].p[0]-spv3.p[0],edge[0].p[1]-spv3.p[1],edge[0].p[2]-spv3.p[2]};
        out_productn(dir1,dir2,tn1);
        out_productn(dir2,dir3,tn2);
        out_productn(dir3,dir1,tn3);
        int re=0;
        if(inner_product(tn1,tn2,3)>=0&&inner_product(tn1,tn3,3)>=0
            &&inner_product(tn2,tn3,3)>=0)
        {
            re=1;
        }
        //free(tn1);free(tn2);free(tn3);
        return re;
    }

    double *tri[3]={spv1.p, spv2.p,spv3.p};
    double t[2][4]={0};
    line_intersection_with_polygon(l,tri,3,t);
    double dis=distance_of_two_points(edge[0].p,edge[1].p,3);

    double t1[2]={0};

    if(double_equal_zero(t[0][3])==0)
    {
        return 0;
    }

    if(double_equal_zero(t[1][3])==0)
    {
        t[1][2]=t[0][2];
    }

    if(t[0][2]<t[1][2])
    {
        t1[0]=t[0][2];t1[1]=t[1][2];
    }
    else
    {
        t1[1]=t[0][2];t1[0]=t[1][2];
    }
    if(t1[0]>dis||t1[1]<0)
    {return 0;}

    return 1;
}

//修改
// static int is_two_edge_intersecting(Sub_Poly_Ver* edge1,Sub_Poly_Ver*edge2)
// {
//     // double dir[3]={edge2[0].p[0]-edge2[1].p[0],edge2[0].p[1]-edge2[1].p[1], edge2[0].p[2]-edge2[1].p[2]};
//     // normalize(dir,3);
//     if((edge1[0].i==edge2[0].i&&edge1[1].i==edge2[1].i)||
//         (edge1[0].i==edge2[1].i&&edge1[1].i==edge2[0].i))
//     {
//         return 1;
//     }
//     else if(edge1[0].i==edge2[0].i||edge1[1].i==edge2[0].i)
//     {
//         return is_point_in_line_segment(edge2[1].p,edge1[0].p ,edge1[1].p);
//     }
//     else if(edge1[0].i==edge2[1].i||edge1[1].i==edge2[1].i)
//     {
//         return is_point_in_line_segment(edge2[0].p,edge1[0].p ,edge1[1].p);
//     }

//     return is_line_segment_intersecting_with_line_segment(edge1[0].p,edge1[1].p,edge2[0].p ,
//         edge2[1].p,NULL);

// }

static void insert_one_edge2_mesh(Sub_Poly_Ver*spvs,int len,double* n, int** s,
    int *s_len,Sub_Poly_Ver* edge)
{

    if(*s_len<=0){return ;}
    int *tri_marks=(int*)malloc(sizeof(int)*(*s_len) );
    memset(tri_marks,0,sizeof(int)*(*s_len));
    for(int i=0;i<(*s_len);i++)
    {
        tri_marks[i]=is_edge_inter_with_triangle(edge,spvs[s[i][0]],
            spvs[s[i][1]],spvs[s[i][2]],n);
        // if(tri_marks[i]==2)
        // {
        //     free(tri_marks);
        //     return ;
        // }
        // if(tri_marks[i]==0)
        // {
        //     if(!is_triangle_clockwise(spvs[s[i][0]],
        //         spvs[s[i][1]],spvs[s[i][2]],n))
        //     {
        //         tri_marks[i]=1;
        //     }
        // }
        //printf("triansmki:%d\n",tri_marks[i]);
    }
//     if(chuli_connected_tria_marks1(s,
//             *s_len,tri_marks)<=0)
//     {
//         free(tri_marks);
//         return ;
//     }
//     struct Sub_Poly_Edge{
//         Sub_Poly_Ver spv1,spv2;
//         int mark;
//     };
//     struct Sub_Poly_Edge*bound_edges=(struct Sub_Poly_Edge*)malloc(sizeof(struct Sub_Poly_Edge)*3*(*s_len) );
//     int bound_edge_len=0;
//     for(int i=0;i<(*s_len);i++)
//     {
//         if(tri_marks[i]==0){continue;}

//         for(int j=0;j<3;j++)
//         {
//             int flag=0;
//             for(int k=0;k< bound_edge_len;k++)
//             {
//                 if(bound_edges[k].spv1.i==spvs[s[i][(j+1)%3]].i&&bound_edges[k].spv2.i==spvs[s[i][j]].i)
//                 {
//                     bound_edges[k].mark+=1;
//                     flag=1;
//                     break;
//                 }
//             }
//             if(!flag)
//             {
//                 bound_edges[bound_edge_len].spv1=spvs[s[i][j]];
//                 bound_edges[bound_edge_len].spv2=spvs[s[i][(j+1)%3]];
//                 bound_edges[bound_edge_len].mark=1;
//                 bound_edge_len++;
//             }
//         }
//     }
// // 提取边界 //
// // 修改如果存在重复点怎么办, 严禁越界
// // 修改，用图论的矩阵来做可以防止重复点越界
//     int bound_edge_len1=0;
//     int flag=1;
//     Sub_Poly_Ver spv0;
//     spv0.i=-1;
//     while(flag)
//     {
//         flag=0;
//         for(int i=bound_edge_len1;i<bound_edge_len;i++)
//         {
//             if(bound_edges[i].mark!=1)
//             {
//                 continue;
//             }
//             if(spv0.i==-1||
//                 (spv0.i==bound_edges[i].spv1.i))
//             {
//                 struct Sub_Poly_Edge temp=bound_edges[bound_edge_len1];
//                 bound_edges[bound_edge_len1]=bound_edges[i];
//                 bound_edges[i]=temp;
//                 spv0=bound_edges[bound_edge_len1].spv2;
//                 bound_edge_len1++;
//                 flag=1;
//                 break;
//             }
//         }
//     }
//     if(bound_edge_len1<=2){free(tri_marks);return;}
//     //printf("end tiqubian jie%d\n",bound_edge_len1);
//     int** s_copy=(int**)malloc(sizeof(int*)*(*s_len));
//     for(int i=0;i<(*s_len);i++)
//     {
//         s_copy[i]=(int*)malloc(sizeof(int)*3);
//         s_copy[i][0]=s[i][0];
//         s_copy[i][1]=s[i][1];
//         s_copy[i][2]=s[i][2];
//     }
//     int s_len1=0;
//     for(int i=0;i<(*s_len);i++)
//     {
//         if(tri_marks[i]==0)
//         {
//             s[s_len1][0]=s_copy[i][0];
//             s[s_len1][1]=s_copy[i][1];
//             s[s_len1][2]=s_copy[i][2];
//             s_len1++;
//         }
//     }
//     int s_len2=*s_len;
//     *s_len=s_len1;
//     Sub_Poly_Ver*bound_polygon=(Sub_Poly_Ver*)malloc(sizeof(Sub_Poly_Ver)*bound_edge_len1);
//     int bound_edge_len2=0;
//     for(int i=0;i<bound_edge_len1;i++)
//     {
//         if(is_subv_in_polygon(bound_edges[i].spv1,bound_polygon,bound_edge_len2))
//         {
//             continue;
//         }
//         bound_polygon[bound_edge_len2]=bound_edges[i].spv1;
//         bound_edge_len2++;
//     }
//     int insert_poly= insert_one_edge2_polygon(spvs,len, n,s,
//         s_len,edge,bound_polygon,bound_edge_len2);
//     if(insert_poly==0)
//     {
//         for(int i=0;i<s_len2;i++)
//         {
//             s[i][0]=s_copy[i][0];s[i][1]=s_copy[i][1];s[i][2]=s_copy[i][2];
//         }
//         *s_len=s_len2;
//     }
//     for(int i=0;i<s_len2;i++)
//     {
//         free(s_copy[i]);
//     }
//     free(bound_polygon);
//     free(s_copy);
//     free(tri_marks);
//     free(bound_edges);
}
void subdivision_of_polygon_with_edges(double** v,int len,
    int* bound,int bound_len,int**edges,int edge_len,
    int **re,int* re_len)
{

    if(len<3){return ;}

    if(len==3)
    {
        if(bound_len<3){return ;}
        else
        {
            re[0][0]=bound[0];re[0][1]=bound[1];re[0][2]=bound[2];
            *re_len=1;
            return ;
        }
    }
    Sub_Poly_Ver* spvs=(Sub_Poly_Ver*)malloc(sizeof(Sub_Poly_Ver)*len);
    for(int i=0;i<len;i++)
    {
        sub_poly_ver_init(&(spvs[i]));
        spvs[i].p=v[i];
        spvs[i].i=i;
    }
    Sub_Poly_Ver* spvs_bound=(Sub_Poly_Ver*)malloc(sizeof(Sub_Poly_Ver)*bound_len);
    double **bound_v=(double**)malloc(sizeof(double*)*bound_len);
    for(int i=0;i<bound_len;i++)
    {
        sub_poly_ver_init(&(spvs_bound[i]));
        spvs_bound[i]=spvs[bound[i]];
        bound_v[i]=v[bound[i]];
    }
    double n[3]={0};
    compute_plane_normal_of_jordan_curve(bound_v,bound_len,n);

    subdivision_of_polygon1(spvs_bound,bound_len,re,n);
    *re_len=bound_len-2;
    //show_sub_data(re,*re_len);


    for(int i=0;i<edge_len;i++)
    {
        Sub_Poly_Ver spvs_edge[2];
        spvs_edge[0]=spvs[edges[i][0]];
        spvs_edge[1]=spvs[edges[i][1]];
        // //printf("insert edge:%d %d\n",edges[i][0],edges[i][1]);
        insert_one_edge2_mesh(spvs,len,n, re,
            re_len,spvs_edge);
    }




    //normalize(n,3);
    free(bound_v);
    free(spvs_bound);
    free(spvs);
}



int** optimization_subdivision_of_polygon_with_edges(double** v,int len,
    int* bound,int bound_len,int**edges,int edge_len,int* re_len)
{


    return NULL;
}
